#include "MMMSimoxTools.h"

#include <MMM/Motion/Legacy/LegacyMotion.h>

#include <VirtualRobot/Robot.h>
#include <VirtualRobot/VirtualRobotException.h>
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualization.h>
#include <VirtualRobot/Visualization/VisualizationNode.h>
#include <VirtualRobot/RuntimeEnvironment.h>
#include <VirtualRobot/Nodes/RobotNodeRevoluteFactory.h>
#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/CollisionDetection/CollisionModel.h>
#include <VirtualRobot/Nodes/RobotNodeFixedFactory.h>
#include <VirtualRobot/Nodes/RobotNodePrismaticFactory.h>
#include <VirtualRobot/Nodes/PositionSensor.h>
#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/RobotFactory.h>

#include <memory>

using std::cout;
using std::endl;

using namespace VirtualRobot;


namespace MMM
{
namespace SimoxTools
{
   
VirtualRobot::RobotPtr buildModel(MMM::ModelPtr model, bool loadVisualizations)
{
	THROW_VR_EXCEPTION_IF(!model, "NULL data");
	std::string robotType = model->getName();
	std::string robotName = robotType;

	VirtualRobot::RobotPtr robo(new VirtualRobot::LocalRobot(robotName, robotType));
	std::vector<MMM::ModelNodePtr> MMMNodes = model->getModels();

	std::vector<VirtualRobot::RobotNodePtr> allNodes;
	std::map< VirtualRobot::RobotNodePtr, std::vector<std::string> > childrenMap;
	MMM::ModelNodePtr rootNode;
	for (auto & MMMNode : MMMNodes)
	{
		if (MMMNode->name == model->getRoot())
		{
			rootNode = MMMNode;
			break;
		}
	}
	THROW_VR_EXCEPTION_IF(!rootNode, "Could not determine root node with name " << model->getRoot());

    VirtualRobot::RobotNodePtr rootVR = convertNode(rootNode, MMMNodes, allNodes, childrenMap, robo, loadVisualizations);

    VirtualRobot::RobotFactory::initializeRobot(robo, allNodes, childrenMap, rootVR);

    for (ModelNodeSet const& ns : model->getModelNodeSets())
    {
        RobotNodeSetPtr rns_ptr = VirtualRobot::RobotNodeSet::createRobotNodeSet(robo,ns.ModelNodeSetName,ns.ModelNodes,ns.rootName,ns.tcpName);
        robo->registerRobotNodeSet(rns_ptr);
    }

	return robo;
}

RobotNodePtr convertNode(MMM::ModelNodePtr modelNode, std::vector<MMM::ModelNodePtr> &MMMNodes, std::vector<VirtualRobot::RobotNodePtr>& allNodes, std::map< VirtualRobot::RobotNodePtr, std::vector<std::string> > &childrenMap, RobotPtr robo, bool loadVisualizations)
{
	RobotNodePtr result;
    VirtualRobot::RobotNodeFactoryPtr prismaticNodeFactory = VirtualRobot::RobotNodeFactory::fromName(VirtualRobot::RobotNodePrismaticFactory::getName(), NULL);
    VirtualRobot::RobotNodeFactoryPtr revoluteNodeFactory = VirtualRobot::RobotNodeFactory::fromName(VirtualRobot::RobotNodeRevoluteFactory::getName(), NULL);
	VirtualRobot::RobotNodeFactoryPtr fixedNodeFactory = VirtualRobot::RobotNodeFactory::fromName(VirtualRobot::RobotNodeFixedFactory::getName(), NULL);

	Eigen::Vector3f idVec3 = Eigen::Vector3f::Zero();
	std::string name = modelNode->name;

	Eigen::Matrix4f preJointTransform = modelNode->localTransformation;

   /* float scaling = 1.0f;
    if (visuScalings.find(name) != visuScalings.end())
        scaling = visuScalings.find(name)->second;
        */

	// simox works with MM
	preJointTransform.block(0, 3, 3, 1) *= 1000.0f;
	VirtualRobot::VisualizationNodePtr rnVisu;
	VirtualRobot::CollisionModelPtr rnCol;
	if (loadVisualizations && !modelNode->segment.visuType.empty() && !modelNode->segment.visuFile.empty())
	{
		std::string visuFactory = modelNode->segment.visuType;
		XML::toLowerCase(visuFactory);
		VisualizationFactoryPtr visualizationFactory = VisualizationFactory::fromName(visuFactory, NULL);
		if (visualizationFactory)
		{
			rnVisu = visualizationFactory->getVisualizationFromFile(modelNode->segment.visuFile);
			if (!rnVisu)
				VR_WARNING << "Could not read file " << modelNode->segment.visuFile << endl;
			else
			{

                if (modelNode->scaling != 1.0f)
                    rnVisu = rnVisu->clone(true, modelNode->scaling);
				rnCol.reset(new CollisionModel(rnVisu));
			}
		} else
			VR_WARNING << "VisualizationFactory of type '" << modelNode->segment.visuType << "' not present. Ignoring Visualization data" << endl;
	}

    VirtualRobot::SceneObject::Physics physics;
    physics.massKg = modelNode->segment.mass;
    physics.inertiaMatrix = modelNode->segment.inertia;
    physics.comLocation = physics.eCustom;
//<<<<<<< HEAD
    physics.localCoM = modelNode->segment.com;
//=======
//    physics.localCoM = modelNode->segment.com * 1000.0f; // scale to mm
//>>>>>>> 93d7bb17764774edc4fcfaf2cfe873b689326186


	switch (modelNode->joint.jointType)
	{
	case eFixed:
		result = fixedNodeFactory->createRobotNode(robo, name, rnVisu, rnCol, 0, 0, 0, preJointTransform, idVec3, idVec3, physics);
		break;
	case eRevolute:
        result = revoluteNodeFactory->createRobotNode(robo, name, rnVisu, rnCol, modelNode->joint.limitLo, modelNode->joint.limitHi, 0, preJointTransform, modelNode->joint.axis, idVec3, physics);
		break;
    case ePrismatic:
        result = prismaticNodeFactory->createRobotNode(robo, name, rnVisu, rnCol, modelNode->joint.limitLo, modelNode->joint.limitHi, modelNode->joint.initValue, preJointTransform, modelNode->joint.axis, idVec3, physics);
        // CHECK if causes problems
        //result->setJointInit(modelNode->joint.initValue);
        //result->setJointValue(modelNode->joint.initValue); // does not work because of a lock
        break;
	default:
        result = fixedNodeFactory->createRobotNode(robo, name, rnVisu, rnCol, 0, 0, 0, preJointTransform, idVec3, idVec3, physics);
        std::cout << std::endl << "RobotNode [" << name << "] has a not implemented joint type: " << modelNode->joint.jointType << std::endl << std::endl;
        //THROW_VR_EXCEPTION("Node type nyi...")
    }

	// sensor/markers
	for (size_t i = 0; i < modelNode->markers.size(); i++)
	{
        SensorPtr s = convertMarker(modelNode->markers[i], result, modelNode->scaling);
		if (!s)
			cout << "Could not build sensor " << i << " at robot node " << result->getName() << endl;
	}

	robo->registerRobotNode(result);
	allNodes.push_back(result);



	std::vector<std::string> childrenModel = modelNode->children;
	childrenMap[result] = childrenModel;
	for (const auto & i : childrenModel)
	{
		MMM::ModelNodePtr childNode;
		for (auto & MMMNode : MMMNodes)
		{
			if (MMMNode->name == i)
			{
				childNode = MMMNode;
				break;
			}
		}
		THROW_VR_EXCEPTION_IF(!childNode, "No Child with name " << i << " found...");
		convertNode(childNode, MMMNodes, allNodes, childrenMap, robo, loadVisualizations);

	}

	return result;
}


VirtualRobot::SensorPtr convertMarker(MMM::MarkerInfoPtr m, VirtualRobot::RobotNodePtr rn, float scaling)
{
	if (!m || !rn)
		return SensorPtr();

	Eigen::Matrix4f lt = m->localTransform;
	lt.block(0, 3, 3, 1) *= scaling*1000.0f; // m -> mm
	SensorPtr s(new PositionSensor(rn, m->name, VirtualRobot::VisualizationNodePtr(), lt));
	rn->registerSensor(s);

	return s;
}

bool updateInertialMatricesFromModels(VirtualRobot::RobotPtr robot)
{
	if (!robot)
		return false;

	float scaling = 0.001f; // mm -> m

	std::vector<RobotNodePtr> robotNodes = robot->getRobotNodes();
	for (auto rn : robotNodes)
	{

			CollisionModelPtr colModel = rn->getCollisionModel();

        Eigen::Matrix3f mZero;
        mZero.setZero();
        if ((rn->getInertiaMatrix() == mZero) && (colModel))
		{
			// get local bbox
			BoundingBox bbox = colModel->getBoundingBox(false);
			Eigen::Vector3f halfExtents = (bbox.getMax() - bbox.getMin())*0.5f;
			float lx = 2.0f*(halfExtents.x()) * scaling;
			float ly = 2.0f*(halfExtents.y()) * scaling;
			float lz = 2.0f*(halfExtents.z()) * scaling;
			Eigen::Matrix3f inertia = Eigen::Matrix3f::Zero();
            inertia(0, 0) = rn->getMass() * 1.0f / 12.0f * (ly*ly + lz*lz);
            inertia(1, 1) = rn->getMass() * 1.0f / 12.0f * (lx*lx + lz*lz);
            inertia(2, 2) = rn->getMass() * 1.0f / 12.0f * (lx*lx + ly*ly);
			rn->setInertiaMatrix(inertia);
		}
	}
	return true;
}

std::vector<std::string> getUnitableSubnodes(RobotNodePtr robotNode, std::vector<std::string> actuatedJoints)
{
    std::vector<std::string> unitableSubnodes;

    std::vector<RobotNodePtr> allDescendants;
    robotNode->collectAllRobotNodes(allDescendants);

    for (std::vector<RobotNodePtr>::const_iterator i = allDescendants.begin(); i != allDescendants.end(); ++i)
    {
        // Skip current robot node
        if (*i == robotNode)
            continue;

        if (std::find(actuatedJoints.begin(), actuatedJoints.end(), (*i)->getName()) != actuatedJoints.end())
        {
            /* MMM_INFO << "Cannot join " << robotNode->getName() << ": Contains joint "
                     << *(std::find(actuatedJoints.begin(), actuatedJoints.end(), (*i)->getName())) << "." << std::endl; */

            // Actuated joint in subtree -> recursive descent
            std::vector<SceneObjectPtr> children = robotNode->getChildren();
            for (std::vector<SceneObjectPtr>::const_iterator j = children.begin(); j != children.end(); ++j)
            {
                RobotNodePtr childRobotNode = std::dynamic_pointer_cast<RobotNode>(*j);
                if (childRobotNode)
                {
                    std::vector<std::string> s = getUnitableSubnodes(childRobotNode, actuatedJoints);
                    unitableSubnodes.insert(unitableSubnodes.end(), s.begin(), s.end());
                }
                /* else
                    MMM_INFO << "Ignoring " << (*j)->getName() << std::endl; */
            }

            return unitableSubnodes;
        }
    }

    // No actuated joint in subtree
    if (!robotNode->getChildren().empty())
        unitableSubnodes.push_back(robotNode->getName());

    return unitableSubnodes;
}

RobotPtr buildReducedModel(RobotPtr fullRobot, std::vector<std::string> actuatedJoints)
{
    std::vector<std::string> unitableSubnodes = getUnitableSubnodes(fullRobot->getRootNode(), actuatedJoints);

    if (unitableSubnodes.empty())
    {
        MMM_INFO << "No robot subnodes can be united by buildReducedModel()." << std::endl;
        return fullRobot;
    }
    else
    {
        std::stringstream ss;
        ss << unitableSubnodes[0];

        for (std::vector<std::string>::const_iterator i = unitableSubnodes.begin() + 1; i != unitableSubnodes.end(); ++i)
        {
            ss << ", " << *i;
        }

        MMM_INFO << "Robot nodes that are united by buildReducedModel(): " << ss.str() << std::endl;

        return RobotFactory::cloneUniteSubsets(fullRobot, "MMMSimoxTools_Reduced_Model", unitableSubnodes);
    }
}

}
}
